﻿using OxyPlot;
using OxyPlot.Axes;
using OxyPlot.Series;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Reflection;
using System.Text;
using System.Text.RegularExpressions;
using System.Threading;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Media;
using System.Windows.Shapes;
using System.Windows.Threading;
using System.Device.Location;

namespace WpfApplication1.Controllers
{
    public class SmartController : IController
    {
        public static double strokethickness = 0.4;
        public double canvasScaleFactor { get; set; }

        private int i, j = 0;
        private ViewModels.SmartViewModel smartViewModel;

        public Connections.UdpServer udpServer { get; private set; }
        public bool MODULATING_PAUSED { get; set; }
        public Modulators.PIDBase PIDRoll { get; set; }
        public Modulators.PIDBase PIDBank { get; set; }
        public Modulators.PIDBase PIDPitch { get; set; }
        public Modulators.PIDBase PIDRadialAcceleration { get; set; }
        public Modulators.PIDBase PIDTurnRate { get; set; }
        public Models.Aircraft aircraft { get; set; }

        public int plotRefreshCounter = 0;

        //test
        public double turnRateOld = 0;

        public double navigationOffsetX { get; set; }
        public double navigationOffsetY { get; set; }
        public double canvasOffsetX { get; set; }
        public double canvasOffsetY { get; set; }
        public Line trajectoryLine { get; set; }
        public Line trajectoryErrorLine { get; set; }
        public Line aircraftLine { get; set; }
        public Line aircraftTrackLine { get; set; }
        public Line accelerationSidewaysLine { get; set; }
        public Line LLine { get; set; }
        public Vector errorVector;
        public Vector a, b, c;

        public Models.Navigation navigation { get; set; }


        //trackCalculations: dev only: old gps data should be in aircraft.class
        //public Queue<Vector> oldGpsData;
        public Vector track;

        //public Queue<double> trackDigits;

        //garbitsch temp
        public List<double> temp;

        //turnrate test

        //messure gps cords in meters
        GeoCoordinate gcTemp1;
        GeoCoordinate gcTemp2;

        public SmartController(ViewModels.SmartViewModel smartViewModel)
        {
            canvasScaleFactor = 1;

            MODULATING_PAUSED = false;
            this.smartViewModel = smartViewModel;

            PIDRoll = new Modulators.PIDBase(100, 1, true);//max error= 0.5, 100 is test only
            PIDRoll.Kp = 0.01;
            PIDRoll.Ki = 0.02;
            PIDRoll.Kd = 0.0001;

            PIDBank = new Modulators.PIDBase(100, 90, false);//max error= 1, 100 is test only
            PIDBank.Kp = 5;
            PIDBank.Ki = 5;
            PIDBank.Kd = 0.5;

            PIDPitch = new Modulators.PIDBase(1, 1, true);

            PIDRadialAcceleration = new Modulators.PIDBase(100, 45, false);

            PIDTurnRate = new Modulators.PIDBase(20, 45, false);


            udpServer = new Connections.UdpServer(this);
            udpServer.startUdpClients();
            aircraft = new Models.Aircraft();
            aircraft.desired_heading = 90;


            //navigation stuff
            navigationOffsetX = 0;
            navigationOffsetY = 0;
            a = new Vector();
            b = new Vector();
            c = new Vector();

            //oldGpsData = new Queue<Vector>();
            //trackDigits = new Queue<double>();


            navigation = new Models.Navigation(0.7d, 2);

            gcTemp1 = new GeoCoordinate();
            gcTemp2 = new GeoCoordinate();
        }

        //called from udpserver
        public String processUdpMessage(String message, double samplingTime)
        {
            PIDRoll.Ta = samplingTime;
            PIDPitch.Ta = samplingTime;
            PIDBank.Ta = samplingTime;
            PIDRadialAcceleration.Ta = samplingTime;
            PIDTurnRate.Ta = samplingTime;
            UdpMessageEncryption.decodeUdpMessage(aircraft, message);
            if (plotRefreshCounter >= 10)
            {
                App.Current.Dispatcher.BeginInvoke(new Action(plotModels));


                plotRefreshCounter = 0;
            }
            else
            {
                plotRefreshCounter++;
            }

                //calc track

                //with queue
                //oldGpsData.Enqueue(new Vector(aircraft.longitude, aircraft.latitude));
                //if(oldGpsData.Count>=2){
                //    oldPosition = oldGpsData.Dequeue();
                //    //track = oldGpsData.Last() - oldPosition;
                //    track = oldGpsData.Last() - oldPosition;
                //    trackDigit = Vector.AngleBetween(zeroTrack, track);
                //    trackDigits.Enqueue(trackDigit);
                //    if (trackDigits.Count >= 10)
                //    {
                //        //temp = trackDigits.ToList();
                //        //temp.Sort();
                //        //trackDigit = temp[4];
                //        trackDigit = trackDigits.Average();
                //        trackDigits.Dequeue();
                //    }
                //    //trackDigit = Math.Acos(zeroTrack * track);
                //    //trackDigit = Math.Acos((zeroTrack.X * track.X + zeroTrack.Y * track.Y) / (Math.Sqrt(zeroTrack.X * zeroTrack.X + zeroTrack.Y * zeroTrack.Y) * Math.Sqrt(track.X * track.X + track.Y * track.Y)));
                //    //trackDigit = trackDigit * 57.2957795;
                //    if (trackDigit < 0)
                //    {
                //        aircraft.track = trackDigit * -1;
                //    }
                //    else
                //    {
                //        aircraft.track = 360 - trackDigit;
                //    }
                //}

                //calc track 2.0


                //wihtout queue

                double startModulation = DateTimeAxis.ToDouble(DateTime.Now);
                //calc turnRate: derivation from heading
                aircraft.turnRate = (aircraft.heading - aircraft.headingOld) / samplingTime;
                aircraft.headingOld = aircraft.heading;

                if (Math.Abs(aircraft.turnRate - turnRateOld) > 0.5)
                {
                    if (aircraft.turnRate - turnRateOld > 0)
                    {
                        aircraft.turnRate = turnRateOld + 0.5;
                    }
                    else
                    {
                        aircraft.turnRate = turnRateOld - 0.5;
                    }
                }
                turnRateOld = aircraft.turnRate;
                //if (aircraft.heading != aircraft.headingOld)
                //{
                //    aircraft.headingOld = aircraft.heading;
                //}


                PIDRoll.speed = aircraft.speed;
                PIDPitch.speed = aircraft.speed;
                PIDBank.speed = aircraft.speed;
                PIDRadialAcceleration.speed = aircraft.speed;
                PIDTurnRate.speed = aircraft.speed;

                //navigation
                //navigation.GroundSpeed = new Vector(Models.Navigation.cosinusDegree(90 - aircraft.track), Models.Navigation.sinusDegree((90 - aircraft.track)));
                navigation.GroundSpeed = new Vector(Models.Navigation.cosinusDegree(aircraft.track), Models.Navigation.sinusDegree((aircraft.track)));
                navigation.GroundSpeed = aircraft.groundspeed * navigation.GroundSpeed;
                //war verkehrt! überprüfe bei zwei zeilen oben "new vector"
                //navigation.GroundSpeed = -1 * navigation.GroundSpeed;

                //problem: groundspeed in m/s and vehicle in lang and long?
                //solve: take actual distance in meters between the navoffset and the gps cords of the uav and scalar it with the normalized vector vehicle
                gcTemp1.Latitude = navigationOffsetX;
                gcTemp1.Longitude = navigationOffsetY;
                gcTemp2.Latitude = aircraft.latitude;
                gcTemp2.Longitude = aircraft.longitude;
                navigation.vehicle = new Vector(aircraft.latitude - navigationOffsetX, aircraft.longitude - navigationOffsetY);
                navigation.vehicle.Normalize();
                navigation.vehicle = gcTemp1.GetDistanceTo(gcTemp2) * navigation.vehicle;


                //navigation.crosstrackError = navigation.vehicle.Length*Math.Cos(Vector.AngleBetween(navigation.vehicle,navigation.Trajectory))*Vector.Divide(navigation.Trajectory,navigation.Trajectory.Length) - navigation.vehicle;
                navigation.calcAccelerationSideways();


                if (!MODULATING_PAUSED)
                {
                //betrag von die error zu bahn
                    PIDRadialAcceleration.e = errorVector.Length - aircraft.zAcceleration * Models.Navigation.sinusDegree(aircraft.bankAngle);

                if (PIDRadialAcceleration.isActive())
                {
                    //aircraft.desired_bankAngle = PIDRadialAcceleration.calculate();
                }

                if (navigation.accelerationSideways > 45)
                {
                    // -1 * weil verkehrt 
                    aircraft.desired_bankAngle = -1*45;
                }
                else if (navigation.accelerationSideways < -45)
                {
                    // -1 * weil verkehrt 
                    aircraft.desired_bankAngle = -1*-45;
                }else
                {
                    // -1 * weil verkehrt 
                    aircraft.desired_bankAngle = -1*navigation.accelerationSideways;
                }


                PIDTurnRate.e = aircraft.desired_turnRate - aircraft.turnRate;
                if (PIDTurnRate.isActive())
                {
                    //aircraft.desired_bankAngle = PIDRadioAcceleration.calculate();//PIDTurnRate.calculate();  // Turn Rate Regler funtkioniert noch nicht richtig zurzeit ausgeschalted
                }

                PIDBank.e = aircraft.desired_bankAngle - aircraft.bankAngle;
                if (PIDBank.isActive())
                {
                    aircraft.desired_rollRate = PIDBank.calculate();
                }

                PIDRoll.e = aircraft.desired_rollRate - aircraft.rollRate;
                if (PIDRoll.isActive())
                {
                   aircraft.desired_aileron= PIDRoll.calculate();
                }

                //Modulators.Autopilot.modulate(aircraft);


                PIDPitch.e = aircraft.desired_pitchRate - aircraft.pitchRate;
                if (PIDPitch.isActive())
                {
                    aircraft.desired_elevator = PIDPitch.calculate() *(-1);
                }

                double endModulation = DateTimeAxis.ToDouble(DateTime.Now);
                aircraft.modulationTime = endModulation - startModulation;
            }


            return UdpMessageEncryption.encodeUdpMessage(aircraft); ;
        }

        public void plotModels()
        {
            //navigation stuff
            if (aircraftLine != null)
            {
                drawAircraft();
            }

            double time = DateTimeAxis.ToDouble(DateTime.Now);
            foreach (String key in smartViewModel.plotModels.Keys)
            {
                PlotModel plotModel;
                if (smartViewModel.plotModels.TryGetValue(key, out plotModel))
                {
                    foreach (LineSeries lineSerie in plotModel.Series)
                    {
                        FieldInfo field;
                        switch (lineSerie.Title)
                        {
                            case "actual":
                                field = typeof(Models.Aircraft).GetField(plotModel.Title);
                                //if (field!=null && field.GetCustomAttribute(typeof(Models.PlotFieldSettings)) == null)
                                //{
                                lineSerie.Points.Add(new DataPoint(time, Convert.ToDouble(field.GetValue(aircraft))));
                                //}
                                break;
                            case "desired":
                                field = typeof(Models.Aircraft).GetField("desired_" + plotModel.Title);
                                //if (field != null && field.GetCustomAttribute(typeof(Models.PlotFieldSettings)) == null)
                                //{
                                lineSerie.Points.Add(new DataPoint(time, Convert.ToDouble(field.GetValue(aircraft))));
                                //}
                                break;
                            default:
                                Console.WriteLine("PMR-Error: An illegal LineSeries was found in a Plotmodel");
                                break;
                        }
                    }
                    plotModel.RefreshPlot(true);
                }
            }
        }

        public void resetNavigationFactors()
        {
            navigationOffsetX = aircraft.latitude;
            navigationOffsetY = aircraft.longitude;
            drawTrajectory();
            smartViewModel.navigationMap.st.CenterX = canvasOffsetX;
            smartViewModel.navigationMap.st.CenterY = canvasOffsetY;
            smartViewModel.navigationMap.st.ScaleX = canvasScaleFactor;
            smartViewModel.navigationMap.st.ScaleY = canvasScaleFactor;
        }

        public void initNavigationMapProperties()
        {
            //navigationMap stuff
            trajectoryLine = new Line();
            trajectoryLine.Stroke = Brushes.Green;
            trajectoryLine.StrokeThickness = strokethickness;
            smartViewModel.navigationMap.NavigationMapCanvas.Children.Add(trajectoryLine);

            aircraftLine = new Line();
            aircraftLine.Stroke = Brushes.Red;
            aircraftLine.StrokeThickness = strokethickness;
            smartViewModel.navigationMap.NavigationMapCanvas.Children.Add(aircraftLine);

            trajectoryErrorLine = new Line();
            trajectoryErrorLine.Stroke = Brushes.Orange;
            trajectoryErrorLine.StrokeThickness = strokethickness;
            smartViewModel.navigationMap.NavigationMapCanvas.Children.Add(trajectoryErrorLine);

            aircraftTrackLine = new Line();
            aircraftTrackLine.Stroke = Brushes.Blue;
            aircraftTrackLine.StrokeThickness = strokethickness;
            smartViewModel.navigationMap.NavigationMapCanvas.Children.Add(aircraftTrackLine);

            accelerationSidewaysLine = new Line();
            accelerationSidewaysLine.Stroke = Brushes.Aqua;
            accelerationSidewaysLine.StrokeThickness = strokethickness;
            smartViewModel.navigationMap.NavigationMapCanvas.Children.Add(accelerationSidewaysLine);

            LLine = new Line();
            LLine.Stroke = Brushes.Black;
            LLine.StrokeThickness = strokethickness;
            smartViewModel.navigationMap.NavigationMapCanvas.Children.Add(LLine);

            canvasOffsetX = smartViewModel.navigationMap.NavigationMapCanvas.ActualWidth / 2;
            //canvasOffsetY = smartViewModel.navigationMap.NavigationMapCanvas.ActualHeight / 2;
            canvasOffsetY = canvasOffsetX;
        }

        public void drawTrajectory()
        {
            canvasOffsetX = smartViewModel.navigationMap.NavigationMapCanvas.ActualWidth / 2;
            //canvasOffsetY = smartViewModel.navigationMap.NavigationMapCanvas.ActualHeight / 2;
            canvasOffsetY = canvasOffsetX;
            String[] pos = Regex.Split(smartViewModel.navigationMap.TrajectoryFunctionTextBox.Text, ",");
            if (pos.Length == 4)
            {
                trajectoryLine.X1 = double.Parse(pos[0]) + canvasOffsetX;
                trajectoryLine.Y1 = double.Parse(pos[1]) + canvasOffsetY;
                trajectoryLine.X2 = double.Parse(pos[2]) + canvasOffsetX;
                trajectoryLine.Y2 = double.Parse(pos[3]) + canvasOffsetY;

                navigation.trajectoryStartPoint = new Vector(trajectoryLine.X1,trajectoryLine.Y1);
                navigation.trajectoryEndPoint = new Vector(trajectoryLine.X2, trajectoryLine.Y2);
                navigation.trajectory = navigation.trajectoryEndPoint - navigation.trajectoryStartPoint;

                
                aircraftLine.X1 = trajectoryLine.X1;
                aircraftLine.Y1 = trajectoryLine.Y1;

                //trajectoryLine.InvalidateVisual();
            }
        }

        public void drawL()
        {
            LLine.X1 = aircraftLine.X2;
            LLine.Y1 = aircraftLine.Y2;
            //LLine.X2 = aircraftLine.X2 + navigation.L.X * canvasScaleFactor;
            //LLine.Y2 = aircraftLine.Y2 + navigation.L.Y * canvasScaleFactor;
            LLine.X2 = aircraftLine.X2 + navigation.L.X;
            LLine.Y2 = aircraftLine.Y2 + navigation.L.Y;
        }

        public void drawAccelerationSideways()
        {
            var temp = Models.Navigation.rotateVector(navigation.GroundSpeed, 90);
            temp.Normalize();
            accelerationSidewaysLine.X1 = aircraftLine.X2;
            accelerationSidewaysLine.Y1 = aircraftLine.Y2;
            //accelerationSidewaysLine.X2 = aircraftLine.X2 + temp.X * navigation.accelerationSideways * canvasScaleFactor;
            //accelerationSidewaysLine.Y2 = aircraftLine.Y2 + temp.Y * navigation.accelerationSideways * canvasScaleFactor;
            accelerationSidewaysLine.X2 = aircraftLine.X2 + temp.X * navigation.accelerationSideways;
            accelerationSidewaysLine.Y2 = aircraftLine.Y2 + temp.Y * navigation.accelerationSideways;
        }

        public void drawAircraft()
        {
            //waypot to aircraft
            //aircraftLine.X2 = canvasOffsetX + navigation.vehicle.X * canvasScaleFactor;
            //aircraftLine.Y2 = canvasOffsetY + navigation.vehicle.Y * canvasScaleFactor;
            aircraftLine.X2 = canvasOffsetX + navigation.vehicle.X;
            aircraftLine.Y2 = canvasOffsetY + navigation.vehicle.Y;

            //error vector
            //if (trajectoryLine.X2 != 0 || trajectoryLine.Y2 != 0)
            //{
            //    b.X = trajectoryLine.X2;
            //    b.Y = trajectoryLine.Y2;

            //    a.X = aircraftLine.X2;
            //    a.Y = aircraftLine.Y2;

            //    c = Vector.Multiply(a*b / b.LengthSquared, b);
            //    trajectoryErrorLine.X1 = a.X;
            //    trajectoryErrorLine.Y1 = a.Y;
            //    trajectoryErrorLine.X2 = c.X;
            //    trajectoryErrorLine.Y2 = c.Y;

            //    errorVector = c - a;
            //}
            trajectoryErrorLine.X1 = aircraftLine.X2;
            trajectoryErrorLine.Y1 = aircraftLine.Y2;
            //trajectoryErrorLine.X2 = aircraftLine.X2 + navigation.crosstrackError.X * canvasScaleFactor;
            //trajectoryErrorLine.Y2 = aircraftLine.Y2 + navigation.crosstrackError.Y * canvasScaleFactor;
            trajectoryErrorLine.X2 = aircraftLine.X2 + navigation.crosstrackError.X;
            trajectoryErrorLine.Y2 = aircraftLine.Y2 + navigation.crosstrackError.Y;

            //track vector
            aircraftTrackLine.X1 = aircraftLine.X2;
            aircraftTrackLine.Y1 = aircraftLine.Y2;
            //aircraftTrackLine.X2 = aircraftLine.X2 + navigation.GroundSpeed.X * canvasScaleFactor;
            //aircraftTrackLine.Y2 = aircraftLine.Y2 + navigation.GroundSpeed.Y * canvasScaleFactor;
            aircraftTrackLine.X2 = aircraftLine.X2 + navigation.GroundSpeed.X;
            aircraftTrackLine.Y2 = aircraftLine.Y2 + navigation.GroundSpeed.Y;

            drawAccelerationSideways();
            drawL();
        }
    }
}
